/*
 * bootloader.c
 *
 * Created: 5/21/2011 8:35:22 PM
 *  Author: X-Ravin
 */ 

#include "kernel.h"

void writeFlashProgram(char progNumber, unsigned int size) 
{	
	//NOTE: writes to word address / 2
	
	uint32_t address = (uint32_t)program[progNumber].offset_address;
	uint32_t aligned;
    char sreg, numPages, data[2];
	unsigned int word, i, j;

    sreg = SREG;
    cli();
    
    eeprom_busy_wait();
	//Determine number of full pages
    numPages = size / SPM_PAGESIZE;
	//Align to previous page
	aligned = address - (address % SPM_PAGESIZE);
	//Copy existing contents we aren't changing
	while(aligned != address)
	{
		boot_page_fill(aligned, pgm_read_word(aligned));
		aligned += 2;
	}
	aligned = address + size;

	boot_page_erase (address);
    boot_spm_busy_wait();

	//Start getting new data over USART to finish first page
	for(; (address % SPM_PAGESIZE) != 0; address += 2)
	{
		data[0] = getchar();
		data[1] = getchar();
		word = ((int)data[1] << 8) + (int)data[0];
		boot_page_fill (address, word);
	}
	
    boot_page_write (address - SPM_PAGESIZE);
    boot_spm_busy_wait();

	for(j = 0; j < numPages; j++)
	{
		for (i = 0; i < SPM_PAGESIZE; i += 2)
     	{
			if(address >= aligned)
			{
				boot_page_fill(address, pgm_read_word(aligned));
			}
			else
			{
				data[0] = getchar();
				data[1] = getchar();
				word = ((int)data[1] << 8) + (int)data[0];
				boot_page_fill (address, word);
			} 
			address += 2;
    	}

		boot_page_erase (address - SPM_PAGESIZE);
    	boot_spm_busy_wait();
        boot_page_write (address - SPM_PAGESIZE);
        boot_spm_busy_wait();
	}

    boot_rww_enable();
	
	program[progNumber].state = RUNNABLE;
	process[program[progNumber].PID].state = RUNNABLE;

	
	//verifyProg(progNumber);
	
    SREG = sreg;

}
